#include "ParticleContact.h"

using namespace Pantsu;

ParticleContact::ParticleContact() 
	:
	m_fRestitution( 0.5f ),
	m_v3ContactNormal( 0.0f, 1.0f, 0.0f )
{
	m_pContactParticles[ 0 ] = 0;
	m_pContactParticles[ 1 ] = 0;
}

ParticleContact::~ParticleContact() {}

void ParticleContact::ResolveCollision( float i_deltaTime ) {

	ResolveVelocity( i_deltaTime );
	ResolveInterpenetration( i_deltaTime );
}

void ParticleContact::ResolveVelocity( float i_deltaTime ) {

	float separatingVelocity = CalculateSeparatingVelocity();

	if ( separatingVelocity > 0.0f ) 
		return;
	
	float newSepVelocity = -separatingVelocity * m_fRestitution;
	float deltaVelocity = newSepVelocity - separatingVelocity;

	float totalInverseMass = m_pContactParticles[ 0 ]->GetMass();
	if ( m_pContactParticles[ 1 ] )
		totalInverseMass += m_pContactParticles[ 1 ]->GetMass();

	if ( totalInverseMass <= 0.0f )
		return;

	float Impulse = deltaVelocity / totalInverseMass;
	
	m_v3ContactNormal *= Impulse;
	Vector3 &impulsePerIMass = m_v3ContactNormal;

	m_pContactParticles[ 0 ]->SetVelocity( m_pContactParticles[ 0 ]->GetVelocity() + impulsePerIMass * m_pContactParticles[ 0 ]->GetMass() );

	if ( m_pContactParticles[ 1 ] )
		m_pContactParticles[ 1 ]->SetVelocity( m_pContactParticles[ 1 ]->GetVelocity() + impulsePerIMass * -m_pContactParticles[ 1 ]->GetMass() );
}

void ParticleContact::ResolveInterpenetration( float i_deltaTime ) {
}

float ParticleContact::CalculateSeparatingVelocity() const {
	
	Vector3 relativeVelocity = m_pContactParticles[ 0 ]->GetVelocity();
	if ( m_pContactParticles[ 1 ] )
		relativeVelocity -= m_pContactParticles[ 1 ]->GetVelocity(); 

	return relativeVelocity * m_v3ContactNormal;
}